#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <termios.h> // 用于终端输入设置

// 全局变量
ros::Publisher vel_pub;
geometry_msgs::Twist cmd_vel;
float linear_vel = 0.5;  // 默认线速度(m/s)
float angular_vel = 1.0; // 默认角速度(rad/s)

// 非阻塞键盘输入函数
char getKey() {
    char key = 0;
    struct termios oldt, newt;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    key = getchar();
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return key;
}

// 打印控制说明
void printInstructions() {
    std::cout << "\nKeyboard Teleop for SLAM Robot\n";
    std::cout << "---------------------------\n";
    std::cout << "W/A/S/D: 控制移动\n";
    std::cout << "Q/E:     增加/减少速度\n";
    std::cout << "Space:   急停\n";
    std::cout << "ESC:     退出\n";
    std::cout << "当前速度: 线性=" << linear_vel << " 角=" << angular_vel << "\n";
    std::cout << "---------------------------\n";
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "joy_term");
    ros::NodeHandle nh;

    // 发布/cmd_vel话题
    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // 初始化指令
    cmd_vel.linear.x = 0;
    cmd_vel.angular.z = 0;

    printInstructions();

    while (ros::ok()) {
        char key = getKey();

        // 处理按键
        switch (key) {
            case 'w': // 前进
                cmd_vel.linear.x = linear_vel;
                cmd_vel.angular.z = 0;
                break;
            case 's': // 后退
                cmd_vel.linear.x = -linear_vel;
                cmd_vel.angular.z = 0;
                break;
            case 'a': // 左转
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = angular_vel;
                break;
            case 'd': // 右转
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = -angular_vel;
                break;
            case 'q': // 加速
                linear_vel += 0.1;
                angular_vel += 0.1;
                printInstructions();
                break;
            case 'e': // 减速
                linear_vel = std::max(0.1f, linear_vel - 0.1f);
                angular_vel = std::max(0.1f, angular_vel - 0.1f);
                printInstructions();
                break;
            case ' ': // 急停
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = 0;
                break;
            case 27: // ESC键退出
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = 0;
                vel_pub.publish(cmd_vel);
                return 0;
            default:
                break;
        }

        // 发布速度指令
        vel_pub.publish(cmd_vel);
        ros::spinOnce();
    }

    return 0;
}